from ossaudiodev import control_names
import rclpy
from rclpy.node import Node
import sys, select, termios, tty
from std_msgs.msg import Float64MultiArray

settings = termios.tcgetattr(sys.stdin)


#      - rotate_joint
#      - raise_joint
#      - extend_joint
#      - pulley_joint
#      - lift_joint

movingBindings = {
    'w': [0.0, 0.0, 0.1, 0.0, 0.0], #EXTEND FOWRAWD
    's': [0.0, 0.0, -0.1, 0.0, 0.0], #EXTEND BACK
    'a': [0.1, 0.0, 0.0, 0.0, 0.0], #ROTATE LEFT
    'd': [-0.1, 0.0, 0.0, 0.0, 0.0], #ROTATE RIGHT
    'u': [0.0, 0.1, 0.0, 0.0, 0.0], #RISE UP
    'j': [0.0, -0.1, 0.0, 0.0, 0.0], #RISE DOWN
    'i': [0.0, 0.0, 0.0,  0.0, 0.1], #LIFT UP
    'k': [0.0, 0.0, 0.0, 0.0,  -0.1], #LIFT DOWN
}

def getKey():
    tty.setraw(sys.stdin.fileno())
    select.select([sys.stdin], [], [], 0)
    key = sys.stdin.read(1)
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

class TeleopKeyboard(Node):

    def __init__(self):
        super().__init__('teleop_keyboard')
        self.declare_parameter("controller_name", "forward_velocity_controller")
        controller_name = self.get_parameter("controller_name").value
        publish_topic = "/" + controller_name + "/" + "commands"
        self.publisher_ = self.create_publisher(Float64MultiArray, publish_topic, 1)
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)
    
    def timer_callback(self):
        msg = Float64MultiArray()
        key = getKey()
        if key in movingBindings.keys():
            msg.data = movingBindings[key]
            self.publisher_.publish(msg)
            self.get_logger().info(f'Publishing: "{msg.data}"')
        else:
            msg.data = [0.0, 0.0, 0.0, 0.0, 0.0]
            self.publisher_.publish(msg)
            self.get_logger().info("Stop")

def main(args=None):
    rclpy.init(args=args)
    teleop_keyboard = TeleopKeyboard()
    rclpy.spin(teleop_keyboard)
    teleop_keyboard.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
